import queue
import threading
import time
import socket
import matplotlib.pyplot as plt
import numpy as np
from myStruct import ObjectInfo
import gpsAndUtm
import motionSim
from algo import my_interpolate

# 和工控机服务器通信
# 每次连接成功 发送身份识别字符串 pathPlan
# 断线自动重连  服务器没上线等待


class MyTcpClient(threading.Thread):

	def __init__(self, get_port, max_len) -> None:
		super().__init__()
		self.identify = "pathPlan"
		# 创建 socket 对象
		self.s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
		# 获取本地主机名
		self.host = socket.gethostname()
		# 设置端口号
		self.port = get_port
		self.maxLen = max_len
		self.cnt_flag = False
		self.msg = ""

	def start_connect(self):
		'''
		开始连接
		'''
		while not self.cnt_flag:
			# 连接服务，指定主机和端口
			time.sleep(1)
			# self.s.connect((self.host, self.port))
			# self.__cntFlag = True
			try:
				self.s.connect((self.host, self.port))
				print("连接成功")
				self.cnt_flag = True
			except ConnectionRefusedError:
				self.cnt_flag = False
				print("等待服务器上线")
			except OSError:
				# 创建 socket 对象
				self.s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
				print("在非套接字上进行了操作 重新创建套接字对象")

	def run(self):
		'''
		线程循环
		循环发送和接收msg
		'''
		self.start_connect()
		# 连接成功 错误指示器为 0
		# 开始循环收发
		while self.cnt_flag:
			self.s.send(str.encode(self.identify, 'utf-8'))
			# 收发要实现分离
			try:
				# 对断包需要进行处理 这是取固定字符串长度的问题
				self.msg = self.s.recv(self.maxLen)
				print("recv msg : " + str(self.msg, 'utf-8'))

				# 路径生成以及发送
				path = PathGen().path_generator()
				self.s.send(str.encode(path, 'utf-8'))
			# 这里recv一直在等待不超过10长度的数据 等收到之后才会向下运行 否则一致阻塞
			# 每个数据片长度固定是10
			except ConnectionAbortedError:
				self.cnt_flag = False
				print("connction lost")
				time.sleep(1)
				self.start_connect()


class PathGen:
	def __init__(self):
		self.q = queue.Queue()
		# 消息队列 线程间通信用
		end_h = 2000
		set_speed = 5
		init_x = np.array([0, 0, 0, 0, 0, 0])
		self.init_lon = 120.30754612708418
		self.init_lat = 31.49450233916176
		# ---插值获得路径 也可用其他方法获得路径
		# ---插值的X 必须是单调增加的
		path_observed_x = np.array([50, 100, 500, 700])
		path_observed_y = np.array([20, 100, 100, 200])
		path_observed_x, path_observed_y = my_interpolate(path_observed_x, path_observed_y, method='cubic')
		self.path = np.zeros(shape=[len(path_observed_x), 3])
		for i in range(len(path_observed_x)):
			self.path[i][0] = path_observed_x[i]
			self.path[i][1] = path_observed_y[i]
		# 原始路径点 n*[北 东 地] 本a地北东地坐标系

		# 生成障碍物位置 这里是从UDP接收
		self.objs = []
		# 测试用障碍物 位置3 速度1 大地运动方向角1 长度1 宽度1 高度1 危险半径1 类型1 n*10矩阵
		obj = ObjectInfo()
		obj.x = np.array([600, 100, 0, 0, 0, 0])
		obj.nu = np.array([1, 1, 0, 0, 0, 0])
		obj.lhw = np.array([20, 20, 10])
		self.objs.append(obj)
		# 设置 最大仿真时间 最大跟踪速度 初始机器人位置
		self.boat_set = [end_h, set_speed, init_x]
		self.mode = 1

	def plot_motion_slot(self, point):
		# 完成循环仿真 开始绘图
		# 在最后进行整体经纬度变换，取北东地坐标系
		'''

		:param point: list[ndarray]
		:return: x-list, y-list
		'''
		x = []
		y = []
		theta = []
		# print('len of points:', len(points))
		for var in point:
			# print('var is :', var)
			# 获取x坐标
			x.append(var[0])
			# 获取y坐标
			y.append(var[1])
			# 获取航向角
			theta.append(var[5])
		# print('x is :', x)
		# print('y is :', y)
		# print('x,y len is :', len(x), len(y))
		return x, y

	def plot_path(self, some_pa):
		'''
		转换数据格式
		:param some_pa:
		:return:
		'''
		x = []
		y = []
		for var in some_pa:
			x.append(var[0])
			y.append(var[1])
		return x, y

	def plot_circle(self, x, y, r):
		'''

		:param x: 圆心坐标x
		:param y: 圆心坐标y
		:param r: 圆半径
		:return: 圆一圈的点
		'''
		a_x = np.arange(0, 2 * np.pi, 0.01)
		a = x + r * np.cos(a_x)
		b = y + r * np.sin(a_x)
		return a, b

	def path_generator(self):
		start_t = time.time()
		# ----------------------------------- BLOCK MOTION SIMULATION START -----------------------------------
		my_mmg = motionSim.MyMotionSim(q=self.q, path=self.path, objs=self.objs, boat_set=self.boat_set, mode=self.mode)

		# 启动线程 开始run
		my_mmg.start()
		while True:
			if not self.q.empty():
				# 消息队列中获取到了值

				break
		# 消息队列先进先出
		points = self.q.get()
		# 等待线程退出
		my_mmg.join()
		# 计算代码所用时间
		end_time = time.time()
		print('MOTION SIMULATION TIME IS:', end_time - start_t)

		points_wgs84 = gpsAndUtm.get_list_wgs84(init_gps84=[self.init_lon, self.init_lat], points_xyz=points)
# -------------------------------------- PLOT START----------------------------------------
		# 绘制第一张图 本地坐标系
		rx, ry = self.plot_motion_slot(points)
		plt.subplot(2, 1, 1)
		# 绘图坐标系和仿真坐标系不一致 改成一致
		plt.plot(ry, rx, label="actual motion path")
		plt.xlabel('east/m')
		plt.ylabel('north/m')
		if self.mode == 1:
			# 路径跟踪
			# print('points is ', points)
			sx, sy = self.plot_path(self.path)
			plt.plot(sy, sx, label="desire path")
			plt.legend()
		elif self.mode == 2:
			# 避障
			for obj in self.objs:
				# 开始避障
				a, b = self.plot_circle(obj.x[0], obj.x[1], obj.danger_r)
				plt.plot(b, a, linestyle='-')
				# 实际障碍半径
				a, b = self.plot_circle(obj.x[0], obj.x[1], 0.3*obj.danger_r)
				plt.plot(b, a, linestyle='-')

		# WGS84坐标
		plt.subplot(2, 1, 2)
		plt.plot(points_wgs84[:, 0], points_wgs84[:, 1], label="WGS84 motion path")
		plt.xlabel('lon/deg')
		plt.ylabel('lat/deg')
		plt.legend()

		plt.show()
# ------------------------------------- PLOT END ------------------------------------------
		return points_wgs84
# ----------------------------------- BLOCK MOTION SIMULATION END -----------------------------------


